//******************************************************************************
//
//                 Low Cost Vision
//
//******************************************************************************
// Project:        EquipletNode
// File:           main.cpp
// Description:    Contains the equipletnode
// Author:         Martijn Beek
// Notes:          ...
//
// License:        GNU GPL v3
//
// This file is part of EquipletNode.
//
// EquipletNode is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// EquipletNode is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with EquipletNode.  If not, see <http://www.gnu.org/licenses/>.
//******************************************************************************
#include <iostream>
#include <stdexcept>

#include "ros/ros.h"
#include <queue>
#include "string.h"
#include "equipletnode/registerNode.h"
#include <equipletnode/Crate.hpp>
#include <BlackboardClient/BlackboardClientUtils.h>
#include <BlackboardClient/PostIt.pb.h>
#include <equipletnode/MiniBall.hpp>
#include <grippernode/grippersrv.h>
#include <deltarobotnode/motionSrv.h>
#include <equipletnode/MotionWrapper.hpp>
#include <vision/getCrate.h>

#include <boost/algorithm/string.hpp>

using namespace BlackboardClient;
using namespace std;

vector<equipletnode::registerNode> listOfNodes;
queue<datatypes::point3f> queueOfCoordinates;
BlackboardClientUtils * bbclient = new BlackboardClientUtils();
datatypes::point3f currentCoordinates;


void nodeRegCallback(const equipletnode::registerNode& rn)
{
	cout << "Received\n";
	listOfNodes.push_back(rn);
	PostItBox * pp  = new PostItBox();
	pp->set_iswrite(true);
	pp->set_zone("BB3");
	PostIt * p = pp->add_postits();
	p->set_payload(rn.action.c_str());
	p->set_owner("equiplet1Agent");
	bbclient->writeToBlackboard(pp);

}

void initiate_agent(string myName)
{
	PostItBox * pp  = new PostItBox();
	pp->set_iswrite(true);
	pp->set_zone("BB4");
	PostIt * p = pp->add_postits();
	p->set_payload(myName);
	p->set_owner("Director");
	bbclient->writeToBlackboard(pp);
}



int main(int argc, char** argv)
{
	ros::init(argc, argv, "equiplet1");
	ros::NodeHandle n;
	ros::Subscriber sub;
	sub = n.subscribe("NodeRegistration", 1000, nodeRegCallback);
	
	initiate_agent("equiplet1Agent");

    ros::Rate loop_rate(1000);

	while(ros::ok())
	{		
		PostItBox * pp  = new PostItBox();
		pp->set_iswrite(false);
		pp->set_zone("BB3");
		pp->set_readowner("equiplet1");
		PostItBox_Filter * f = pp->mutable_filter();
		f->set_filtername("PostItFilter");	
		pp = bbclient->readFromBlackboard(pp);

		if(pp->postits_size() > 0)
		{
			int i,j;
			cout << pp->postits_size();
			
			for(i = 0; i < pp->postits_size();i++)
			{
				for(j = 0; j < pp->postits_size();j++)
				{
					if(pp->postits(j).ordinal() == i)
					{
						PostIt p = pp->postits(j);
						string s = p.payload();
						if(s.find("GRIPPER")!=string::npos)
						{					 
							//GRIPPER (1/0)
				
							 ros::ServiceClient client = n.serviceClient<grippernode::grippersrv>("enableGripper");
							 grippernode::grippersrv srv;
							 size_t pos;
							 pos = s.find(" ");
							 s = s.substr(pos+1);
							 if(atoi(s.c_str()) > 0)
							 {
								  srv.request.enabled = true; //the stuff after gripper;
								  cout << "gripper on";
							 }
							 else
							 	  srv.request.enabled = false;
							 cout << "gripper:" << s;
							 if (client.call(srv))
							 {
							  	cout << srv.response.succeeded << "\n";
							 }				
						}
						else if(s.find("VISION")!=string::npos)
						{
					
							//VISION qr
							std::vector<CrateContent*> vect;
							ros::ServiceClient motionClient = n.serviceClient<vision::getCrate>("getCrate");
							vision::getCrate getCrate;
							size_t pos;
							int positionno;
							 pos = s.find(" ");
							 s = s.substr(pos+1);		

							std::vector<std::string> strs;					
							boost::split(strs, s, boost::is_any_of("\t "));
					
		
							getCrate.request.name = strs.at(0);
							positionno = atoi(strs.at(1).c_str());
					


							if(motionClient.call(getCrate))
							{
						
								datatypes::point3f crateLocation;
								datatypes::point2f location = datatypes::point2f (getCrate.response.crate.x,getCrate.response.crate.y); 
								for(size_t i = 0; i < 4*4; i++)
								{
									vect.push_back(new MiniBall(datatypes::BLUE));
								}
						
								Crate * c = new Crate(getCrate.response.crate.name,location,getCrate.response.crate.angle,false,vect );
						
						
								crateLocation = c->getContentGripLocation(positionno);
						
								cout << crateLocation.z;
								queueOfCoordinates.push(crateLocation);
						
						
							}
					
				
						}
						else if(s.find("DELTA")!=string::npos)
						{
						cout<<"\ndelta\n";
							//DELTA x y z
							ros::ServiceClient motionClient = n.serviceClient<deltarobotnode::motionSrv>("moveTo");
							ros::ServiceClient checkClient = n.serviceClient<deltarobotnode::motionSrv>("checkTo");
				
							deltarobotnode::motionSrv motions;
							size_t pos;
							 pos = s.find(" ");
							 s = s.substr(pos+1);
							 					 
							std::vector<std::string> strs;
							MotionWrapper motionToSourceUp;
							boost::split(strs, s, boost::is_any_of("\t "));
							if(!(strs.size() < 3))
							{	
								if(strs.size() == 4)
								{
									if(!queueOfCoordinates.empty())
									{
										cout << "OFFSET AND read from vision";	
										currentCoordinates = datatypes::point3f(queueOfCoordinates.front().x+atof(strs.at(0).c_str()),queueOfCoordinates.front().y+atof(strs.at(1).c_str()),queueOfCoordinates.front().z+atof(strs.at(2).c_str()));
											queueOfCoordinates.pop();
									}
									
									else
									{
										cout << "OFFSET NO  vision";	
										currentCoordinates = datatypes::point3f(currentCoordinates.x+atof(strs.at(0).c_str()),currentCoordinates.y+atof(strs.at(1).c_str()),currentCoordinates.z+atof(strs.at(2).c_str()));
									
									}					
									motionToSourceUp.addMotion(currentCoordinates,36);
								
								}
								else	
								{			
									cout << "COORDS only";
									currentCoordinates = datatypes::point3f(atof(strs.at(0).c_str()), atof(strs.at(1).c_str()), atof(strs.at(2).c_str()));	
									motionToSourceUp.addMotion(currentCoordinates, 36);
								}
					
							}
							else
							{
								cout << "VISION only";
								
									currentCoordinates = datatypes::point3f(queueOfCoordinates.front().x,queueOfCoordinates.front().y,queueOfCoordinates.front().z);
								   motionToSourceUp.addMotion(currentCoordinates,36);
								   	queueOfCoordinates.pop();
							
							}
						
							
							if(motionToSourceUp.callService(checkClient))
							{
								motionToSourceUp.callService(motionClient);					
							}				
							
				
						}
					}
				}			
			}
		}
		sleep(1);
		loop_rate.sleep();
		ros::spinOnce();
	}
   

	return 0;
}

